#include "StdAfx.h"
#include "Protocol_CT.h"
#include "Serial.h"

//async_read doesn't work with std::bind
#include "boost/bind.hpp"

// Enumerate FrSky packet codes
static const uint8_t FRSKY_LINK_PACKET	= 0xfe;
static const uint8_t FRSKY_USER_PACKET	= 0xfd;
static const uint8_t FRSKY_A11_PACKET	= 0xfc;
static const uint8_t FRSKY_A12_PACKET	= 0xfb;
static const uint8_t FRSKY_A21_PACKET	= 0xfa;
static const uint8_t FRSKY_A22_PACKET	= 0xf9;
static const uint8_t FRSKY_ALARM_REQUEST	= 0xf8;
static const uint8_t FRSKY_RSSI_RX_PACKET	= 0xf7;
static const uint8_t FRSKY_RSSI_TX_PACKET	= 0xf6;
static const uint8_t FRSKY_RSSI_REQUEST	= 0xf1;
												
static const uint8_t FRSKY_START_STOP	= 0x7e;
static const uint8_t FRSKY_BYTESTUFF	= 0x7d;
static const uint8_t FRSKY_STUFF_MASK	= 0x20;
static const uint8_t FRSKY_PRIVATE		= 0x1B;

static const size_t FRSKY_FRAME_SIZE	= 10;

//////////////////////////////////////////////////////////////////////////

Protocol_CT::Protocol_CT()
	: m_io()
	, m_port(m_io)
	, m_packet_loss(0)
	, m_packet_count(0)
	, m_telemetry_packet_size(0)
{
	memset(&m_data, 0, sizeof(Data));

	m_link_packet_count = 0;
	m_user_packet_count = 0;
}
Protocol_CT::~Protocol_CT()
{
	m_port.close();
	m_io.stop();
}

void Protocol_CT::init(const std::string& com_device)
{
	m_port.open(com_device);
	m_port.set_option(boost::asio::serial_port_base::baud_rate(9600));
	m_port.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
	m_port.set_option(boost::asio::serial_port_base::character_size(8));
	m_port.set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
	m_port.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));

	read_async();
}

void Protocol_CT::serial_callback(const boost::system::error_code& error, size_t bytes_transferred)
{
	{
		std::lock_guard<std::mutex> lg(m_buffer_mutex);
		m_buffer.insert(m_buffer.end(), m_serial_buffer.begin(), m_serial_buffer.begin() + bytes_transferred);
	}

	//m_serial_buffer[bytes_transferred] = 0;
	//std::cout << bytes_transferred << " bytes: " << m_serial_buffer.data() << std::endl;

	read_async();
}

void Protocol_CT::read_async()
{
	m_port.async_read_some(boost::asio::buffer((char*)m_serial_buffer.data(), m_serial_buffer.size() - 1),
		boost::bind(&Protocol_CT::serial_callback, 
		this,
		boost::asio::placeholders::error, 
		boost::asio::placeholders::bytes_transferred));
}

void Protocol_CT::update()
{
	if (m_io.poll_one())
	{
		m_io.run_one();
	}

	parse_data();

	{
		static uint32_t l = m_packet_loss;
		if (l != m_packet_loss)
		{
			l = m_packet_loss;
			std::cout << "packet loss: " << m_packet_loss << " packet count: " << m_packet_count << std::endl;
		}
	}
}

size_t Protocol_CT::find_header_offset(const std::vector<uint8_t>& buffer, size_t offset)
{
	for (size_t i = offset; i < buffer.size(); i++)
	{
		if (buffer[i] == FRSKY_START_STOP)
		{
			return i;
		}
	}
	return static_cast<size_t>(-1);
}

void Protocol_CT::parse_data()
{
	std::lock_guard<std::mutex> lg(m_buffer_mutex);

	while (true)
	{
		size_t start = find_header_offset(m_buffer, 0);
		if (start == static_cast<size_t>(-1))
		{
			if (!m_buffer.empty())
			{
				m_packet_loss++;
			}
			m_buffer.clear();
			break;
		}
		//it might be the end of the header, so skip again to the next 7E
		if (start + 1 < m_buffer.size() && m_buffer[start + 1] == FRSKY_START_STOP)
		{
			start++;
		}

		size_t end = find_header_offset(m_buffer, start + 1);
		if (end == static_cast<size_t>(-1))
		{
			break;
		}

		if (end - start < FRSKY_FRAME_SIZE)
		{
			m_packet_loss++;
			m_buffer.clear();
			break;
		}

		m_packet.clear();
		m_packet.reserve(end - start);
		bool stuffing = false;
		for (size_t i = start + 1; i < end; i++)
		{
			uint8_t b = m_buffer[i];
			if (b == 0x7D) 
			{
				stuffing = true;
				continue;
			} 
			if (stuffing) 
			{
				b ^= 0x20;
				stuffing = false;
			}
			m_packet.push_back(b);
		}

		bool ok = false;
		uint8_t packet_type = m_packet[0];
		switch (packet_type)
		{
		case FRSKY_LINK_PACKET: ok = parse_link_packet(); break;
		case FRSKY_USER_PACKET: ok = parse_user_packet(); break;
		case FRSKY_A11_PACKET: ok = true; break;
		case FRSKY_A12_PACKET: ok = true; break;
		case FRSKY_A21_PACKET: ok = true; break;
		case FRSKY_A22_PACKET: ok = true; break;
		case FRSKY_ALARM_REQUEST: ok = true; break;
		case FRSKY_RSSI_RX_PACKET: parse_rssi_rx_packet(); break;
		case FRSKY_RSSI_TX_PACKET: parse_rssi_tx_packet(); break;
		case FRSKY_RSSI_REQUEST: ok = true;
		default: break;
		}

		if (!ok)
		{
			m_packet_loss++;
		}

		m_buffer.erase(m_buffer.begin() + start, m_buffer.begin() + end + 1);
	}
}

bool Protocol_CT::parse_user_packet()
{
	uint8_t length = m_packet[1] & 0x7;
	uint8_t b[8] = {0};
	uint8_t off = 3;
	for (uint8_t i = 0; i < length && i + off < m_packet.size(); i++)
	{
		b[i] = m_packet[off + i];
		m_all_telemetry_data.push_back(b[i]);
	}
	b[length] = 0;

	size_t idx = 0;
	m_all_telemetry_data.push_back(1000);

	while (idx < length)
	{
		if (m_telemetry_packet_size == 0) //new packet
		{
			m_telemetry_packet.clear();
			if ((b[idx] & 0xC0) != 0xC0) //signature
			{
				return false;
			}
			m_telemetry_packet_size = b[idx] & 63;
			idx++;
		}
		else
		{
			m_telemetry_packet.push_back(b[idx++]);
			if (m_telemetry_packet.size() == m_telemetry_packet_size)
			{
				m_telemetry_packet_size = 0;
				try
				{
					parse_telemetry_packet();	
				}
				catch(const std::exception& e)
				{
					return false;
				}
			}
		}
	}

	return true;
}
bool Protocol_CT::parse_link_packet()
{
	m_link_packet_count++;

	auto now = Clock::now();
	if (now - m_link_packet_last_time > std::chrono::milliseconds(1000))
	{
		m_link_packet_last_time = now;
		//m_data.rx_rtti.update_frequency = m_link_packet_count;
		m_link_packet_count = 0;
	}

	m_data.rx_rssi = m_packet[3];
	m_data.tx_rssi = m_packet[4] / 2;

	//std:: cout << " rx: " << m_rx_rtti.value << "/" << m_rx_rtti.update_frequency << "   tx: " << m_tx_rtti.value << std::endl;

	return true;
}
bool Protocol_CT::parse_rssi_tx_packet()
{
	//assert(end - start == FRSKY_FRAME_SIZE);
	return true;
}
bool Protocol_CT::parse_rssi_rx_packet()
{
	//assert(end - start == FRSKY_FRAME_SIZE);
	return true;
}

void Protocol_CT::parse_telemetry_packet()
{
	//header1
	static const uint8_t k_header1_current = 1 << 0;
	static const uint8_t k_header1_voltage = 1 << 1;
	static const uint8_t k_header1_altitude = 1 << 2;
	static const uint8_t k_header1_motors1234 = 1 << 3;
	static const uint8_t k_header1_roll = 1 << 4;
	static const uint8_t k_header1_pitch = 1 << 5;
	static const uint8_t k_header1_yaw = 1 << 6;
	static const uint8_t k_header1_header2 = 1 << 7;

	uint8_t header1 = m_telemetry_packet.at(0);
	uint8_t header2 = 0, header3 = 0, header4 = 0;
	uint8_t idx = 1;

	if (header1)
	{
		if (header1 & k_header1_current)
		{
			m_data.current = m_telemetry_packet.at(idx++) / 10.f;
		}
		if (header1 & k_header1_voltage)
		{
			m_data.voltage = m_telemetry_packet.at(idx++) / 15.f;
		}
		if (header1 & k_header1_altitude)
		{
			uint8_t alt1 = m_telemetry_packet.at(idx++);
			uint8_t alt2 = m_telemetry_packet.at(idx++);
			uint16_t alt3 = alt1 | (alt2 << 8);
			float alt = (float)(*((int16_t*)&alt3));
			m_data.altitude = alt / 10.f;
		}
		if (header1 & k_header1_motors1234)
		{
			m_data.motors.output[0] = (m_telemetry_packet.at(idx++) & 255) / 255.f;
			m_data.motors.output[1] = (m_telemetry_packet.at(idx++) & 255) / 255.f;
			m_data.motors.output[2] = (m_telemetry_packet.at(idx++) & 255) / 255.f;
			m_data.motors.output[3] = (m_telemetry_packet.at(idx++) & 255) / 255.f;
		}

		const float PI = 3.1415f;
		if (header1 & k_header1_roll)
		{
			m_data.attitude.roll = m_telemetry_packet.at(idx++) * PI * 2.f / 255.f;
		}
		if (header1 & k_header1_pitch)
		{
			m_data.attitude.pitch = m_telemetry_packet.at(idx++) * PI * 2.f / 255.f;
		}
		if (header1 & k_header1_yaw)
		{
			m_data.attitude.yaw = m_telemetry_packet.at(idx++) * PI * 2.f / 255.f;
		}
	}

	//header2
	static const uint8_t k_header2_battery_remaining = 1 << 0;
	static const uint8_t k_header2_header3 = 1 << 7;

	//second header?
	if (header1 & k_header1_header2)
	{
		header2 = m_telemetry_packet.at(idx++);

		if (header2 & k_header2_battery_remaining)
		{
			m_data.capacity = m_telemetry_packet.at(idx++) / 100.f;
		}
	}

	//header2
	static const uint8_t k_header3_rc_input12 = 1 << 0;
	static const uint8_t k_header3_rc_input34 = 1 << 1;
	static const uint8_t k_header3_rc_input56 = 1 << 2;
	static const uint8_t k_header3_rc_input78 = 1 << 3;
	static const uint8_t k_header3_control_mode = 1 << 4;
	static const uint8_t k_header3_status = 1 << 5;
	static const uint8_t k_header3_header4 = 1 << 7;

	//third header?
	if (header2 & k_header2_header3)
	{
		header3 = m_telemetry_packet.at(idx++);

		if (header3 & k_header3_rc_input12)
		{
			auto x = m_telemetry_packet.at(idx++);
			m_data.rc_input.channels[0] = ((x >> 4) & 0xF) / 16.f;
			m_data.rc_input.channels[1] = ((x     ) & 0xF) / 16.f;
		}
		if (header3 & k_header3_rc_input34)
		{
			auto x = m_telemetry_packet.at(idx++);
			m_data.rc_input.channels[2] = ((x >> 4) & 0xF) / 16.f;
			m_data.rc_input.channels[3] = ((x     ) & 0xF) / 16.f;
		}
		if (header3 & k_header3_rc_input56)
		{
			auto x = m_telemetry_packet.at(idx++);
			m_data.rc_input.channels[4] = ((x >> 4) & 0xF) / 16.f;
			m_data.rc_input.channels[5] = ((x     ) & 0xF) / 16.f;
		}
		if (header3 & k_header3_rc_input78)
		{
			auto x = m_telemetry_packet.at(idx++);
			m_data.rc_input.channels[6] = ((x >> 4) & 0xF) / 16.f;
			m_data.rc_input.channels[7] = ((x     ) & 0xF) / 16.f;
		}

		if (header3 & k_header3_control_mode)
		{
			auto x = m_telemetry_packet.at(idx++);
			m_data.control_mode = static_cast<Control_Mode>(x);
		}
		if (header3 & k_header3_status)
		{
			static union 
			{
				struct 
				{
					uint8_t home_is_set         : 1; // 0
					uint8_t simple_mode         : 1; // 1    // This is the state of simple mode
					uint8_t manual_attitude     : 1; // 2
					uint8_t manual_throttle     : 1; // 3

					uint8_t pre_arm_rc_check    : 1; // 5    // true if rc input pre-arm checks have been completed successfully
					uint8_t pre_arm_check       : 1; // 6    // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
					uint8_t auto_armed          : 1; // 7    // stops auto missions from beginning until throttle is raised
					uint8_t logging_started     : 1; // 8    // true if dataflash logging has started

					uint8_t low_battery         : 1; // 9    // Used to track if the battery is low - LED output flashes when the batt is low
					uint8_t failsafe_radio      : 1; // 10   // A status flag for the radio failsafe
					uint8_t failsafe_batt       : 1; // 11   // A status flag for the battery failsafe
					uint8_t failsafe_gps        : 1; // 12   // A status flag for the gps failsafe
					uint8_t failsafe_gcs        : 1; // 13   // A status flag for the ground station failsafe
					uint8_t rc_override_active  : 1; // 14   // true if rc control are overwritten by ground station
					uint8_t do_flip             : 1; // 15   // Used to enable flip code
					uint8_t takeoff_complete    : 1; // 16
					uint8_t land_complete       : 1; // 17
					uint8_t compass_status      : 1; // 18
					uint8_t gps_status          : 1; // 19
				};
				uint32_t value;
			} ap;

			ap.value = m_telemetry_packet.at(idx++) << 24;
			ap.value |= m_telemetry_packet.at(idx++) << 16;
			ap.value |= m_telemetry_packet.at(idx++) << 8;
			ap.value |= m_telemetry_packet.at(idx++);

			m_data.status.simple_mode = ap.simple_mode;
			m_data.status.home_set = ap.home_is_set;
			m_data.status.low_battery = ap.low_battery;
			m_data.status.failsafe_radio = ap.failsafe_radio;
			m_data.status.failsafe_battery = ap.failsafe_batt;
			m_data.status.is_logging = ap.logging_started;
			m_data.status.auto_altitude = ap.manual_attitude == 0;
			m_data.status.auto_throttle = ap.manual_throttle == 0;
			m_data.status.is_armed = (ap.value & (1 << 31)) != 0; //this is added by me
		}
	}


// 	std::cout << m_data.voltage << "V, " << m_data.current << "A, " << m_data.battery_remaining << "%, " << std::endl;
// 	std::cout << "RC: " 
// 		<< (int)(10.f * m_data.rc_input.channels[0]) << "\t" << (int)(10.f * m_data.rc_input.channels[1]) << "\t"
// 		<< (int)(10.f * m_data.rc_input.channels[2]) << "\t" << (int)(10.f * m_data.rc_input.channels[3]) << "\t"
// 		<< (int)(10.f * m_data.rc_input.channels[4]) << "\t" << (int)(10.f * m_data.rc_input.channels[5]) << "\t"
// 		<< (int)(10.f * m_data.rc_input.channels[6]) << "\t" << (int)(10.f * m_data.rc_input.channels[7]) << std::endl;
// 	std::cout 
// 		<< "\troll: "	<< (int)(m_data.attitude.roll * 360.f / 3.1415f) 
// 		<< "\tpitch: "	<< (int)(m_data.attitude.pitch * 360.f / 3.1415f) 
// 		<< "\tyaw: "	<< (int)(m_data.attitude.yaw * 360.f / 3.1415f) 
// 		<< "\tALT: " << m_data.altitude << std::endl;

}

auto Protocol_CT::get_data() const -> Data
{
	return m_data;
}
uint32_t Protocol_CT::get_packet_loss() const
{
	return m_packet_loss;
}

float Protocol_CT::get_update_frequency() const
{
	return 0;
}